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ABSTRACT 

We  present  a  method  for  estimating  the  position  and  orientation  of  a  ground  vehicle  in  an 
environment  with  landmarks.  From  the  geometric  relationships,  we  derive  a  set  of  linear 
equations  with  a  quadratic  constraint,  which  forms  the  basis  for  our  optimisation  problem.  We 
also  extend  the  problem  to  associating  two  sets  of  measurements  taken  at  two  successive  locations 
to  improve  the  navigation  accuracy.  This  method  is  efficient  and  the  performance  is  robust 
against  large  measurement  errors. 
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Landmark-based  Navigation  of  an  Unmanned 
Ground  Vehicle  (UGV) 


Executive  Summary 

We  present  an  efficient  method  for  localising  an  unmanned  ground  vehicle  (UGV)  in  a  two 
dimensional  environment  with  landmarks.  We  assume  that  the  vehicle  can  identify  these 
landmarks  and  measure  their  bearings.  We  derive  a  set  of  linear  equations  that  relate  the 
UGV's  position  and  orientation  to  the  measured  bearings,  assuming  perfect 
measurements.  Having  noise  in  the  measurements,  we  establish  an  estimator,  based  on 
minimising  a  constrained  quadratic  cost  function. 

Landmark-based  navigation  is  useful  where  GPS  is  not  adequate  because  of  signal 
blockage  and  interference.  Possible  defence  applications  include  robots  performing  the 
following  tasks:  indoor  bomb  search,  landmine  location,  autonomous  surveillance  of 
military  bases  or  nuclear  sites,  and  logistics  in  urban  battle  zones. 

The  proposed  algorithm  is  efficient  and  the  performance  is  consistent  given  that  the  UGV 
is  inside  the  boundary  formed  by  the  landmarks.  The  estimation  accuracy  is  shown  to 
improve  as  the  number  of  landmarks  increases.  Given  that  the  number  of  landmarks  is 
limited,  we  extend  the  initial  formulation  and  propose  another  method  that  utilises  two 
sets  of  measurements  from  two  locations  and  estimates  the  current  position  and  azimuth 
with  significantly  improved  accuracy.  The  performance  is  robust  against  large 
measurement  errors,  and  can  be  coupled  with  an  inertial  navigation  system  to  enable 
higher  rate  navigation  with  greater  accuracy,  especially  in  outdoor  settings. 
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1.  Introduction 


This  report  presents  the  problem  of  estimating  the  position  and  azimuth  of  a  mobile  robot  or 
unmanned  ground  vehicle  (UGV),  by  measuring  bearing  angles  to  landmarks.  We  derive  a  set 
of  linear  homogenous  equations  that  relate  the  robot  position  and  orientation  to  the  measured 
bearings.  Given  that  bearing  measurements  are  noisy  and  redundant,  we  establish  an 
estimator  based  on  minimising  a  constrained  quadratic  cost  function. 

Localisation  is  of  interest  for  many  industrial  (e.g,,  robots)  and  defence  applications  (e.g., 
UAVs  and  UGVs).  In  open  outdoor  environments,  differential  GPS  systems  can  provide 
precise  position  information.  However,  there  are  scenarios  where  GPS  is  not  adequate  such  as 
indoor,  underwater,  extraterrestrial  or  urban  environments,  because  of  signal  blockage  and 
multipath  interference.  Also,  for  stand-alone  systems,  the  position  accuracy  may  not  be 
sufficient  [Borenstein  et  al,  1997].  In  defence  applications,  the  GPS  receiver  on  the  ground  level 
is  likely  to  be  subject  to  jamming  or  spoofing  from  an  adversary.  A  simple  alternative  would 
be  odometry  or  inertial  navigation  system  (INS).  However,  both  of  which  are  subject  to  build¬ 
up  of  error  with  time,  and  often  require  an  external  aid. 

Landmark  based  localisation  is  attractive  in  a  controlled  environment  where  the  landmarks 
can  be  precisely  located.  Possible  applications  include  robots  performing  one  or  more  of  the 
following  tasks:  indoor  bomb  search,  landmine  location,  autonomous  surveillance  of  military 
bases  or  nuclear  sites,  and  logistics  in  urban  battle  zones. 

Wre  assume  that  the  robot  is  equipped  with  an  omni-directional  camera  (e.g.,  catadioptric 
sensor,  ringcam,  panospheric  camera)  [Daniilidis,  K,  web  source]. 


Figure  1:  Examples  of  omni-directional  cameras  [Daniilidis,  K ,  web  source] 


The  report  is  organised  as  follows.  In  Section  2,  we  present  the  formulation  of  the  basic 
algorithm  and  optimisation,  and  performance  assessment.  In  Section  3,  we  extend  the  problem 
by  taking  two  sets  of  measurements  at  two  successive  locations  and  combining  them  to 
improve  the  localisation  accuracy.  Section  4  summarises  the  findings  and  concludes  the 
report. 
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2.  Formulation  of  Basic  Algorithm 


2.1  Notations 


XA :  vector  X  in  frame  A  -  (note  that  without  A,  default  frame  is  REF  which  is  a  fixed 
reference  frame). 

X/ 


L  = 


:  ilh  landmark  position  vector 


»■  = 


X 


Bi 


1  Bi 


=  B, :  robot  position  at  i*  time  instance. 


R  A  :  2x2  rotation  matrix  for  coordinate  transformation  from  frame  A  to  frame  B. 


Rotation  Matrices  (and  shorthand  notations): 


B 1  __ 

COS0J 

sin  #, 

c 

R  ~ 

-sin#, 

cos#, 

- 

B2  _ 

cos#2 

sin  #2 

R 

-sin  #2 

cos  #2 

B\  _ 

cos(A#) 

sin(A#) 

B2  ~ 

-sin(A#) 

i  cos(A#) 

REF  to  Robot  at  Position  1 


REF  to  Robot  at  Position  2 


s 

c 


Robot  2  to  Robot  1 


Note  all  the  angular  terms  are  positive  when  measured  counter-clockwise. 

Given  a  robot  positioned  at  BI,  consider  an  i*  landmark  position  L*  expressed  in  body  frame 
(see  Figure  2), 


L?*  = 


Li,cos/?„ 

A,  sin  Pu 


~  Rtf'  (L,  -  B, )  =  Rr'L,  -  Rfl'B,  = 


cos^. 


sin  0,  II  Xu 


-  sin  0.  cos  0. 


+  T/ 


(1) 


where  T,m 


XT 

yt 


=  -R*B,. 


(2) 
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2.2  Derivation  of  Linear  Equations 


V 


LANDMARK 

1 


11=(X,  .  Y,  ) 


LANDMARK 

2 

L2 -(Xt .  V  i 


LANDMARK 

3 

L3=(X;j.  V,  0 


Yfu) 


BODY 

FRAME 


X 


©i  is  negative  in  this  case 


REF  X 

FRAME 


Figure  2:  Illustration  of  a  mobile  robot  that  detects  three  known  landmarks  along  the  directions 
/3U  (3n  /?n  .  The  problem  is  to  find  the  position  B1  and  orientation  #,  of  the  robot  with 
respect  to  REF  frame. 


Dividing  the  top  row  by  bottom  row  gives 

cos /?,,  __  cos0xX u +'s>\x\6xYu  +  XT 
sin  J3U  -  sin  6X  Xu  +  cos  6X  Yu  +  Yr 

This  can  be  rewritten  in  vector  form  as 

cos#, 
sin#, 

XT 

yt 

Let 

AW,  =  sin PUXU  -cos phYlt 
A\2,  =  sin  pxiYu  +cosf3uXu. 


[sin  f$XiXLi  -  cos  PUYU ,  smPvYu+cosPuXu,  sin  pu,  -cos/?,,]' 


(3) 


In  the  absence  of  measurement  errors,  populating  (3)  for  all  N  landmarks  will  produce  N 
homogenous  equations  which  can  be  expressed  as  a  vector  equation 
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Allt  A\2t  sin/?,,  -cos/?,. 

cos#. 

A\\2  A122  sin /?,2  -cos/?,2 

sin#, 

M  M  M  M 

A\\n  A\2n  sin/?1/v  -cos/?l/v 

l 

_ 1 

We  express  the  above  equation  as  Ax  =  b  where  A  is  the  measurement  matrix,  x  contains  the 
three  unknowns  we  want  to  estimate  (ie,.  6X9XT9YT\  and  b  =  0^xl.  If  the  bearing 

measurements  and  landmark  positions  are  error  free,  then  the  exact  solution  to  (4)  can  be 
found  with  N- 3,  as  long  as  the  landmarks  and  the  robot  do  not  lie  on  a  circle  or  a  straight  line 
[Shimshoni,  I.,  2002].  If  the  measurements  are  noisy  and  more  landmarks  are  available,  then 
we  need  to  establish  a  cost  function  and  minimise  it  to  solve  the  localisation  problem. 


2.3  Optimal  Solution 


Note  that  the  matrix  A  contains  the  bearing  measurements.  If  the  measurements  are  noisy, 
then  b  will  no  longer  be  equal  to  0wxl .  The  objective  is  to  minimise  the  cost  function  defined  as 

(  Ax)'  Ax  which  would  typically  be  non-zero. 

We  want  to  minimise 

x7Px  (5) 

subject  to 

x7Qx-l  =  0 . 


where 


P  =  A'  A,  andQ  = 


1  0 
0  1 
0  0 
0  0 


0  0 
0  0 
0  0 
0  0 


The  constraint  x7  Qx  —  1=0  expresses  the  known  property,  cos*  #,  +  sin2  #,  —  1 . 

Now  define  the  Lagrangian  of  (5)  as 

L(x,A)  =  x7Px-/l(x7Qx-l)  (6) 

The  minimiser  of  Equation  6  must  satisfy  the  first  optimality  condition  (7)  and  the  constraint 
(8)  shown  below 


8L 

dx 

dL_ 

dA 


=  2(P  -  AQ)x  =  0 
=  x7  Qx  -1=0. 


(7) 

(8) 
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Solving  (7)  and  (8)  becomes  a  generalised  eigen-decomposition  (GED)  problem.  The  optimal 
Lagrange  multiplier  X  is  the  smallest  positive  eigenvalue  of  the  matrix  pair  (P,Q)  and  x*  is 
the  corresponding  eigenvector,  normalised  according  to  (8). 

2.4  Implementation  Issues 

This  formulation  bears  resemblance  to  the  method  proposed  by  [Shimshoni,  2002].  Shimshoni 
applies  a  number  of  pre-conditioning  processes  to  the  linear  equations  to  arrive  at  the  same 
equation  as  in  (4).  The  main  difference  is  that  the  constraints  used  by  Shimshoni  is  x  x  =  1 
instead  of  x7  Qx  -1=0,  and  therefore  SVD  is  employed  to  solve  the  localisation  problem. 


It  should  be  noted  that  for  numerical  stability,  it  is  often  necessary  to  translate  and  scale  the 
original  geometry  so  that  the  entries  of  A  are  numerically  comparable  in  size. 


First,  we  shift  the  origin  of  the  REF  frame  to  a  mean  of  all  the  landmark  positions,  and  then 
j  )  l  N 

replace  L,with  L/  where  L;  =  L; - =  (N  is  the  number  of  landmarks).  We  then 


A^T 


compute  the  norms  |'L;|j  (ie,  distance  of  each  landmark  from  the  new  origin),  find  the 

maximum  norm  (named  SF),  and  divide  £,by  SF.  This  process  limits  the  position  terms 
within  ±  1 ,  hence  making  them  comparable  to  the  magnitude  of  sine  and  cosine  terms  in  (3). 


When  the  optimal  solution  is  found,  we  need  to  convert  T,  n(=  x’(3  : 4))  to  B{  (ie.,  robot 
position  in  REF  frame)  via  following  transformation. 


B,  =-SF*(R*l)7T®1  +^ZLj 


(9) 


When  the  SVD  or  GED  problem  is  solved,  the  solution  is  a  unit  eigenvector  whose  magnitude 
and  sign  need  to  be  adjusted.  First  we  have  to  satisfy  x(l)2  +  x(2)2  =  1 ,  by  dividing  x’  by 
norm(x*  (1 :  2)) .  To  resolve  the  sign  issue,  we  perform  the  following  routine. 

*****  Algorithm  ******** 

1.  compute  the  robot  azimuth  (0X )  and  position  ( B, )  from  x*. 

2.  given  L^Bj  and  6X ,  the  expected  bearing  j3h  is  computed. 

3.  For  all  landmarks,  compare  the  computed  bearings  with  the  measured  bearings.  If  the 
majority  of  the  "computed-measured"  bearing  pairs  are  well  aligned,  then  accept  the 

solution.  If  the  majority  of  them  are  off  by  about  180°,  then  change  the  sign  of  x’  and 
recompute  (9). 
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2.5  Performance  Comparisons  with  Shimshoni's  Method 

We  reproduced  the  work  of  Shimshoni  to  use  as  a  benchmark.  Here  we  show  that  our  GED- 
based  optimisation  performs  consistently  better  than  the  benchmark,  and  exclude  SVD 
method  in  the  performance  comparison  in  Section  3.5. 

We  tested  both  methods,  using  two  sets  of  4  and  8  landmarks  located  at 

L={(0,  0),  (40,  0),  (0,  40),  (40,  40)}  and 

L={(0,  0),  (40,  0),  (0,  40),  (40,  40),  (20,  0),  (0,  20),  (40,  20),  (20,  40)}. 

We  conducted  1000  Monte-Carlo  simulations  to  get  the  statistics  for  the  estimation  errors. 
Note  the  robot  azimuth  is  set  to  be  uniformly  distributed  between  -180°  and  180°.  The 
bearing  error  is  assumed  to  be  Gaussian  with  standard  deviations  of  3°  by  default.  Robot 
position  is  fixed  at  (4, 10). 

2.5.1  Effects  of  Number  of  Landmarks 


Using  4  landmarks,  bearing  std=3° 

As  shown  in  Figure  3,  both  methods  generate  error  distribution  Gaussian-like  histograms  of 
zero  mean. 

SVD  G6D 


IhMA  MHF  t 


Figure  3:  Histogram  of  x,  y  position  and  azimuth  errors  (from  top  to  bottom)  using  SVD  (left)  and 
GED  (right)  optimisations ,  zvith  bearing  error  standard  deviation  of  3°.  Four  landmarks  are 
used. 

The  comparisons  of  the  estimation  errors  between  the  SVD  and  GED  methods  are  given  as 
RMS  (root  mean  square)  values  in  Table  1.  The  GED  method  produces  smaller  errors  but  the 
difference  is  not  significant. 
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Table  1:  Estimation  errors  (rms)  obtained  using  both  methods  with  four  landmarks.  The  smaller 
numbers  are  shaded  with  pale  yellow. 


RMS  error 

SVD 

GED 

X  position  Error 

0.8452 

0.8444 

Y  position  Error 

1.5839 

1.5780 

Azimuth  Error  (°) 

1.6239 

1.6218 

Using  8  landmarks,  bearing  std=3° 


When  the  bearing  measurements  are  increased  to  8,  the  error  distribution  curves  become  more 
peaked  (see  Figure  4).  Again,  the  performance  difference  is  very  small. 


8VD  Q£D 


Figure  4:  Histogram  ofx,  y  positions  and  azimuth  errors  (from  top  to  bottom)  using  SVD  (left)  and 
GED  (right)  optimisations >  with  bearing  error  standard  deviation  of  3°.  8  landmarks  are 
used. 


Table  2:  Estimation  errors  (rms)  when  eight  landmarks  are  used 


RMS  error 

SVD 

GED 

X  position  Error 

0.5832 

0.5822 

Y  position  Error 

0.9037 

0.8969 

Azimuth  Error  (°) 

1.3764 

1.3744 

It  is  clear  that  the  estimation  accuracy  of  both  methods  improve  when  more  measurements  are 
available. 
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2.5.2  Effect  of  Measurement  Noise 


Here,  wesetN=4,  and  vary  the  bearing  measurement  error  (lcr)froml0  to  10°.  We  introduce 
the  Cramer-Rao  Lower  Bound  (CRLB)  which  is  the  smallest  possible  error  bound  that  any 
unbiased  method  could  achieve  for  the  same  N,  cr(/?)  and  robot-landmark  geometry.  We 
present  the  error  curves  for  the  GED/SVD  along  with  the  CRLB  (see  Figure  5)  to  show  how 
close  they  are  to  the  CRLB  for  various  <j({3)  values.  In  Figure  5,  the  gaps  among  the  three 
curves  are  small  for  <j({3)  up  to  5°.  As  the  bearing  error  becomes  larger  the  gaps  with  the 
CRLB  become  wider.  The  GED  method  appears  to  outperform  the  SVD  method  even  though 
the  differences  remain  small. 


Figure  5:  Plots  of  position  and  azimuth  errors  versus  bearing  errors  (ler) .  The  red  and  blue  curves 
represent  the  rms  errors  from  the  SVD  and  GED  methods  respectively.  The  green  curves 
represent  the  Cramer-Rao  Lower  Bounds. 


Figure  6  shows  the  estimation  error  differences  between  the  SVD  and  GED  methods.  The 
values  are  mostly  positive  indicating  that  the  GED  method  outperforms  consistently.  The 
error  separation  widens  as  the  <r(/?)  increases,  indicating  the  GED  method  is  more  resilient 
against  measurement  noise. 


x  position  srror  gsp 


bssring  srr  (dog) 


y  position  srror  gsp  szimuth  srror  gsp  (dsg) 


bssring  srr(dsg)  bssring  srr  (dsg) 


Figure  6:  Estimation  error  differences  between  the  SVD  and  GED  estimation  methods  as  functions  of 
bearing  error.  A  positive  value  means  the  error  from  the  SVD  method  is  larger  than  the 
error  from  the  GED  method. 
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2.5.3  Effect  of  Landmarks  and  Robot  Geometry 

The  findings  in  Sections  2.5.1  and  2.5.2  are  based  on  the  fixed  robot  position  of  (4, 10).  If  the 
estimation  accuracy  varies  drastically  as  the  robot  position  varies,  then  the  validity  of  the 
previous  results  may  not  hold. 

In  this  subsection,  we  place  the  robot  at  13  different  locations  and  examine  how  the  geometry 
affects  the  performance.  In  Figure  7,  four  landmarks  are  placed  on  the  corners  of  a  square  (in 
green),  and  the  robot  positions  are  varied  from  the  middle  of  the  square  to  well  outside  the 

square.  The  combined  position  errors  (y jx _err 2  +  y  _err2 )  and  azimuth  errors  achieved 
from  those  positions  are  presented  in  Table  3.  The  second  row  shows  the  coordinates  of  the  13 
locations,  and  the  \cr  estimation  errors  are  in  the  3rd  and  4th  rows. 

It  is  observed  that  the  errors  are  small  when  the  robot  is  within  the  square  region  (yellow 
shade  in  Table  3).  Moving  away  from  the  centre  does  not  cause  noticeable  increase  in  the 
estimation  errors  as  long  as  the  robot  is  kept  within  the  perimeter.  As  the  robot  reaches  the 
perimeter,  the  error  increase  becomes  somewhat  more  evident  (grey  shade).  Once  outside  the 
perimeter,  then  the  errors  become  large  and  they  increase  rapidly  as  the  robot  moves  further 
away  from  the  landmarks.  The  optimisation  seems  to  work  best  if  bearings  are  widely 
scattered.  As  the  robot  moves  further  away  from  the  square,  the  bearings  aggregate  in  one 
direction.  This  kind  of  geometric  arrangement  (eg.,  position  13)  seems  to  flatten  the  cost  curve, 
making  the  minimum  prone  to  the  measurement  noise.  Of  course,  this  spread  can  be  reduced 
if  the  measurement  noise  is  reduced  making  the  cost  curve  smoother. 

Another  interesting  observation  is  the  large  jump  in  the  error  at  positions  8  and  9.  This  is  the 
case  when  the  landmarks  and  robot  form  a  circle  accidentally.  In  this  case,  an  infinite  number 
of  solutions  exist.  The  robot  can  be  anywhere  on  the  circle  (shown  in  cyan)  and  receive  the 
same  bearing  measurements  [Betke,  M.,  and  Gurvits,  L.,  1997]. 

In  summary,  localisation  is  most  accurate  when  the  robot  operates  within  the  boundary 
formed  by  the  landmarks. 
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Figure  7: 
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With  four  landmarks  forming  a  square ,  13  different  robot  positions  are  selected  to  examine 
the  effect  of  the  robot  position  on  the  estimation  accuracy 


Table  3:  The  position  and  azimuth  errors  achieved  at  13  different  robot  locations 


index 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

12 

13 

coords 

20,20 

10,20 

20,10 

10,10 

5,5 

20,0 

10,0 

20,-8.3 

10,  -10 

20,  -20 

20, -40 

20,-70 

20,-100 

d  Err 

1.45 

1.54 

1.57 

1.54 

1.68 

2.81 

2.83 

39.7 

9.83 

5.43 

7.47 

15.57 

26.49 

az  Err 

1.51 

1.47 

1.46 

1.51 

1.65 

2.11 

2.39 

51.38 

11.99 

6.10 

5.84 

7.67 

9.53 
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2.6  Nonlinear  Least  Square  Estimation  -  Fine  Tuning 


The  accuracy  of  the  above  estimation  can  be  improved  by  considering  an  alternative  objective 
function  that  directly  compares  the  computed  bearings  to  the  measured  bearings.  For  a  given 
robot  position  and  orientation,  the  bearing  angle  to  the  ith  landmark  is  derived  as 


f 


P,  =/,(*)  =  tan' 


cos0(Xu-XB)  +  s\n0(Yu-YB) 
-sin  0(Xu-Xb)  +  cos0(Yu-Yb)_ 


(10) 


where  \  =  [6,X  and  /  =  IK  N  .  Note  that  this  function  is  non-linear. 
In  practice  the  measured  bearing  is  contaminated  with  additive  noise  as 


p, =/(x)+", 


(ii) 


With  N  measurement,  we  stack  up  all  bearing  measurements  to  form  a  TV  x  1 
measurement  vector 


P  =  f«+n  (12) 

The  problem  objective  is  to  find  x  that  minimises  the  non-linear  cost  function 

Q(x)  =  [p-f(x)]'N-,[p-f(x)]  (13) 

where 

N  =  £[(n  -  £’[n])(n  -  £[n])r  ]  is  the  measurement  error  covariance  matrix.  (14) 

Note  that  if  the  bearing  noise,  n ,  is  Gaussian,  then  the  above  estimator  becomes  the  Maximum 
Likelihood  Estimator  (MLE).  In  order  to  utilise  the  MLE,  the  estimate  from  the  SVD/GED 
method  is  set  as  the  initial  estimate  x0for  the  MLE,  and  a  new  x  is  found  via  a  gradient 
descent  approach.  The  detailed  formulation  is  given  in  Appendix  B.  Figure  8  shows  further 
reduction  of  the  estimation  error  using  the  MLE  -  the  red  curves  are  brought  closer  to  the 
green  curves. 
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bearing  an  (dag)  baarlng  an  (dag)  baanng  an  (dag) 


Figure  8:  The  estimation  errors  from  GED  method  (flue),  GED  method  followed  by  MLE  (green), 
and  CRLB(red)  obtained  using  4  measurements 


We  repeated  the  test  using  8  landmarks  to  see  the  extent  of  the  improvement.  In  Figure  9,  the 
red  curves  are  now  much  closer  to  the  green  curves  (CRLB)  than  in  Figure  8. 


baarlng  an  (dag)  baarlng  art  (dag)  baarlng  an  (dag) 


Figure  9:  The  estimation  errors  of  GED  method  (blue),  GED  method  followed  by  MLE  (green),  and 
CRLB(red)  when  8  landmarks  are  used 
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3.  Extension  to  Two  Measurement  Association  (TMA) 

Problem 


In  practice  there  may  be  limitations  in  the  number  of  landmarks  that  one  can  use.  Several 
reasons  may  contribute  to  this  and  include  the  cost  of  placing  more  landmarks,  increase  in 
image  processing  burden,  occlusion  in  cluttered  environment,  or  distant  landmarks  outside 
the  visible  range. 

Having  visual  sensors,  we  have  to  deal  with  the  possibility  that  only  a  small  number  of 
landmarks  are  visible  by  the  robot  at  any  given  moment.  However,  as  demonstrated  in 
Section  2,  localisation  accuracy  improves  with  the  number  of  measurements.  Therefore,  if  a 
vehicle  can  store  more  than  one  set  of  measurements  at  different  locations  and  if  these 
measurements  can  be  combined  in  some  way,  then  position  estimation  often  becomes  more 
accurate.  This  idea  is  explored  in  the  next  subsection. 

3.1  Geometry  Involving  Two  Successive  Robot  Positions 

Figure  10  shows  the  vehicle's  current  position  B1  and  its  previous  position  B2  We  assume  that 
the  path  between  B2  and  B1  is  circular  (including  straight,  equivalent  to  infinite  radius).  The 
vehicle  can  generate  a  curved  path  via  wheel  steering  [Green,  D.  N.,  et.  al.,  1993]  or 
differentially  driving  the  wheels  [Chong,  K.  S.,  and  Kleeman,  L.,  1997]  [Papadopoulos,  E.,  and 
Misailidis,  M.,  2007]. 

What  we  want  to  estimate  is  {#, ,  XT ,  Yr }  at  B1  (refer  to  (2)  for  definition).  As  in  Section  2.2,  we 
use  the  measurements  taken  at  B1  to  form  the  equation  in  (4).  The  next  step  is  to  correspond 
the  measurements  obtained  from  B2  (ie.,  /?2, )  to  those  collected  at  Bl.  To  do  so  we  introduce 
three  additional  parameters,  dn,  A0  and  a  (see  Figure  10  and  Figure  11)  where  (d]2,a)  is 
the  polar  coordinate  of  B2  with  respect  to  Bl,  and  A0  reflects  the  curvature  of  the  path. 
Mathematical  derivations  of  these  terms  are  given  in  Section  3.2. 
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LANDMARK 

3 


X 

Figure  10:  Illustration  of  a  mobile  robot  moving  from  B2  to  B1  in  1  second  interval.  Rotation  of 
Ad  =  6X  -02  is  the  result  of  taking  a  circular  path. 

3.2  Vehicle  Odometry 

We  use  Figure  11  to  illustrate  the  basic  geometry  leading  to  the  definition  and  derivation  of 
the  three  terms  dn,A6  and  a  .  We  approximate  the  path  between  two  successive  positions  as 
circular.  This  also  includes  straight  paths,  which  are  circular  with  infinite  radius. 
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A&=e,-e2 


Figure  11:  Definition  of  geometric  terms  between  tzoo  successive  vehicle  positions  -  dX2,  AO  and  a 

The  odometry  terms  are  defined  as  below. 

•  a]2  -arc  length  between  B1  and  B2  (circular  arc), 

•  d]2  -  linear  distance  between  B1  and  B2, 

•  a  -  look  angle  to  B2,  and 

•  AO  =  0\  -  02  -  rotation  increment. 

These  terms  can  be  deduced  from  either  wheel  steering  angle  (for  steered  vehicle)  or  wheel 
speeds  (for  differentially  driven  vehicle) 


3.2.1  Wheel  Steered  Vehicle 

Knowing  the  steering  angle  (ie.,  the  control  input),  the  turn  radius  R  (in  Figure  11)  can  be 


computed  using  R  - - where  Land  y  are  the  wheel  base  and  steering  angle  respectively 

tany 


[Green,  D.  N.,  et.  al ,  1993].  Having  computed  R,  the  values  for  dl2 ,  A  0  and  a  can  be  obtained 
as  below. 


(15) 


c/l2  =  Ryj 2(1  -  cos(A#))  and 


(16) 


A0 


a  =  — 


2 


(17) 
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3.2.2  Differentially  Driven  Vehicle 

For  a  differentially  driven  vehicle,  the  control  input  to  the  vehicle  is  u  =  [VR9VL]T ,  where 
VR  and  VL  are  the  linear  speeds  of  the  right  and  left  wheels.  Assuming  that  VR  and  VL  are 
constant  within  each  time  step,  the  dynamic  equation  of  motion  can  be  expressed  as  below 
[Chong,  K.  S.,  and  Kleeman,  L.,  1997]. 


X,' 

X7 

du  cos(02  -  a) 

Yb> 

= 

r,2 

+ 

di2  sin(t?2  -  a) 

X . 

.*2 

A0 

where 


V  -V 

A9  =  — - -,  where  L  is  wheelbase. 


At  and 


A0 


a  - 


3.3  Problem  Formulation 

As  in  Section  2.2,  for  the  current  position  Bl,  we  have 


AW, 

A\2, 

sin/?,, 

-cos/?,, 

a\\2 

A\22 

sin  /?,2 

-cos/?,2 

M 

M 

M 

M 

aun 

A\2n 

sin 

-cos  ft  N 

cost?, 

sint?. 

XT 

.  YT  . 

=  0 


Wxl 


where 

.411,  =  sm  PUXU- cos  PX,YU 
A\2i  =siny0„4  +  cos  ftuXLI 

Now  consider  the  landmark  position  seen  from  the  location  B2. 


LB2  = 


L2i  cos  ft 
L2i  sin  fi2l 


-  R  1-  P  52p5,t 


(19) 

(20) 
(21) 


(22) 


(23) 

(24) 


(25) 
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After  some  algebraic  manipulations,  we  obtain 


[,421,  A22i  sin  Pltc-  cos  filts  -sin  P^s -cos  fi2ic 


cos(0, ) 
sin(0, ) 

Vt 


=  m. 


where 

A2\t  =  [sin/?2j(cA’J,1  -sYu)  + cos  P2i(-sXLi - cYu )] 
A22 ,  =  [s\n  P2,{sX  Li  +cYu)  +  cos/32i(cXLi  -J^,)] 


m,  =-sin P2ic-dnca  -sin P2,s-dx2sa  +cos P2is-dnca  -cos P2,c-d]2sa 


In  the  absence  of  measurement  errors,  stacking  up  for  all  the  landmarks  gives 


',421, 

A  22, 

sin  /?2,c-cos/?2,.j 

-sin/?2,s  -cos/?2,c 

cos($, ) 

A2\2 

A222 

sin/?22c-cos  P22s 

-sin/?22.j-cos  p22c 

sin(#,) 

m2 

M 

M 

M 

M 

XT 

M 

A21n 

A22n 

sin  p2Nc  -  cos  P2Ns 

-sin  P2Ns  -cos P2Nc 

|  rr  . 

_mN_ 

(26) 


(27) 

(28) 
(29) 


(30) 


The  equations  (22)  and  (30)  can  be  combined  in  to  Ax  =  b  where  A  is  2N  x  4 ,  x  is  4x1  and 
b  is  2N  x  1  (see  below). 


'/111, 

A\2i 

sin/?,. 

—  eos  /?, , 

'  0  ' 

A\  lj 

A\22 

sin  /?,2 

—  COS  /?|2 

0 

M 

M 

M 

M 

COS0, 

M 

aun 

A\2n 

sin/?,  w 

—  COS  /?,  (Y 

sin^ 

0 

A2\t 

A22{ 

sin/?2,c-cos/?2,.j 

-  sin  P2ts  -  cos  P2lc 

Xr 

w, 

A2\2 

A222 

sin  P22c  -  cos  P22s 

-sin P22s  -cos P22c 

i 

1 _ 

m2 

M 

M 

M 

M 

M 

A2\n 

A22n 

sin  P2Nc  -cos  P2Ws 

-  sin  P2Ns  -  cos  P2Nc_ 

_mN  _ 

Note  that  A0  is  embedded  in  A  and  b,  and  dn and  a  are  embedded  in  b,  effectively 
replacing  B2  parameters. 
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3.4  Optimal  Solution 

In  presence  of  measurement  errors,  where  Ax  =  b,  our  objective  becomes 

Minimise  x  Px  +  2p  x  +  w 
subject  to  x  Qx  - 1  =  0 


where  P  =  A 7  A  (symmetric  and  positive  definite), 
p'=-brA, 
u  =  b'  b  and 

"l  0  0  0' 

0  10  0 

Q  = 

0  0  0  0 
0  0  0  0 


(32) 


(33) 

(34) 

(35) 

(36) 


To  solve  this  problem,  we  first  transform  (32)  into  a  simpler  form  where  P  disappears  from 
the  quadratic  term. 

Since  P  and  Q  are  symmetric,  and  P  is  positive  definite,  there  exists  a  transformation  matrix 
S  that  will  transformPto  I4x4  and  Qto  a  diagonal  matrix.  (ie„  SPS  =I4x4and 
SQS‘  =  Qn  (diagonal)).  The  proof  is  included  in  Appendix  C. 


S  and  Q/;are  computed  as  follows: 
i  i 

1.  S,  =  P~2  =  Vp£p2Vpr  where  (V„Er)  =  eig( P) 

2.  Let  Y  =  S,QSf 

3.  S2  =  Vy  (eigenvectors  of  Y)  where  (Vv ,  £ v )  =  eig(Y) 

4.  S  =  S2S, 

5.  Qp=SQSr=Lv 


Note  QD  and  Q  are  congruent  matrices,  hence  have  the  same  inertia  (ie.,  same  number  of 
positive,  negative  and  zero  eigenvalues).  They  both  have  2  positive  and  2  zero  eigenvalues. 


Q  !>  = 


0 

0 

0 


0 

Mi 

0 

0 


0 

0 

0 

0 


0 

0 

0 

0 


where  /r,  >  jj2 


(37) 
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Then  the  objective  function  and  constraint  can  be  re-expressed  as  below. 
X  Px  +  2p‘  X  +  W 

=  y'  SPSry  +  2prS'  y  +  u ,  where  \r  =  y‘  S  and  x  =  S'y 
=  y;y  +  2g'  y  +  u ,  where  p‘  ST  =  g' 


xrQx-l 
=  yrSQSry-l 

=  y'Qny-1  =  o 

Now  define  Lagrangian  of  the  reduced  problem  as 
£(y>  ^ = y  ry + 2g 7  y + «  -  A(y  rQ0y  - 1) 

Setting  the  partial  derivative  terms  to  zero. 


—  =  2(y  +  g-AQny)  =  2((I-AQ/,)y  +  g)  =  0 

dy 

3L  T 

—  =  y  Q,.y-i  =  ° 

From  (41), 

y  =  -(I-AQ;,)-'g  =  -D,g 
where 


D,  =(I-AQ0)-' 


1 


1-A/r, 

0 

0 

0 


0  0  0 


1  _ 

•-^2 

0 

0 


0  0 


=  D 


T 

A 


1  0 
0  1 


Substituting  y  =  -D^g  into  (42), 


^)=§  =  grD’iQ„D1g-l  = 

oA 


0  which  can  be  simplified  to 


K(A)  = 


thK  L  |  Ml 


=  0 


(38) 


(39) 

(40) 

(41) 

(42) 

(43) 


(44) 


(45) 
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This  constraint  function  K(X)  can  be  shown  to  be  monotonously  increasing  for  A  within  the 
1 


interval  A  = 


-00. 


My 


,  by  noting  — ^  >0  V  A  €  A  .  Proof  that  the  global  minimum  is 


dX 


within  A  is  shown  in  Appendix  D. 


The  above  finding  combined  with  lim  K(A)  =  -\  and  lim  K(X)  =  +oo  suggests  that 
K{X)  has  exactly  one  zero  crossing  within  A. 


We  can  use  matlab  function / zero  to  look  for  the  zero  crossing  (ie.,  X )  in  A . 


K=@ (L)  C1A2/ (Ml* ( 1 - L*M1 ) A2) +C2A2/ (M2* (1-L*M2) A2) + ( s-Cl "2/M1-C2 A2/M2 ) ; 
L__op=f  zero(K,  1/M1*0 .999)  ; 


Having  found  the  optimal  Lagrange  multiplier  X ,  the  optimal  state  can  be  computed  as 
follows. 


y*  =-(i-A’QD)-lg 


-1 

0 

0 

0 

-z, 

z, 

1-/1>1 

1-A>, 

0 

-1 

0 

0 

Z  2 

-Zi 

1  -  a>2 

Z) 

1-A>2 

0 

0 

-1 

0 

_Z 4  _ 

-Zi 

0 

0 

0 

-1 

.  ~Za 

=>  x’ 


cost?,’ 
sin  O' 

XT 

Y ' 

iT 


sry 


(44) 


(45) 


Optimal  azimuths  and  positions  are  given  below 

<?,*  =  tan~'(x*(2)/x*(l))  (matlab  function  " atan2(x(2),  x(l))") 

e2' =  O' -A6 


K 


L^‘, 


(46) 


(47) 
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y* 

i  1 B  2 


=  B1  + 


d]2COs(*  +  0l  + 

d]2  sin^  +  tf,*  +a) 


(48) 


3.5  Experimental  Result 

3.5.1  Default  Scenario 

The  followings  are  the  default  settings  used  in  the  simulation 

•  landmark  positions:  L={(0,  0),  (40,  0),  (0,  40),  (40,  40)} 

•  speed=5m/ s,  wheel  steering  angle=5°,  wheel  base=0.5m,  At  =lsec. 

•  B2=(4, 10)m  (start),  Bl=  (77, 13.1)m  (end); 

•  <92=15°  (start),  0X  =65.1273°  (end), 

•  dn  =  4.83m,  AO  =  50.13°, a  =  25.06° 

The  relative  geometry  terms  have  Gaussian  noises  as  below: 

•  cr(c/12)=0.25m  (5%  error), 

•  a(A0)=l°  and 

•  cr(or)=l°. 

Default  measurement  noise  is 

•  ct(/?/  )  =  3°for  each  bearing. 

3.5.2  Comparison  with  GED  method 

The  measurement  noise  is  varied  from  0.5°  to  10°.  In  Figure  12,  position  error  (ie,, 
y  x  _  err  +  y  _  err 1  )  and  azimuth  errors  from  the  GED  (blue)  and  TM A  (green)  methods  are 
compared. 
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Figure  12:  Position  and  azimuth  error  curves  by  GED  (blue)  and  TMA  (red). 


It  was  previously  shown  in  Figure  8  that  the  GED  (with  MLE)  error  curves  are  already  close  to 
the  CRLB,  indicating  not  much  room  for  improvement.  To  go  beyond  the  bounds,  we 
somehow  need  to  generate  more  measurements  from  the  given  number  of  landmarks,  which 
is  the  underlying  idea  of  the  TMA  approach.  Having  implemented  this  approach,  the  error 
curves  for  the  TMA  method  (red)  are  significantly  lower  than  the  blue  curves,  and  the 
estimation  improvement  of  25%  and  35%  are  achieved  for  the  position  and  azimuth 
respectively. 

With  the  default  setting  of  3°  bearing  error,  the  position  and  azimuth  errors  are  1.2m  (lcr)  and 
1°  (lcr)  respectively,  and  they  increase  linearly,  reaching  4.5m  and  3.7°  at  10°  bearing  error. 
The  difference  between  the  GED  and  TMA  errors  are  also  presented  in  Figure  13  for  a  closer 
look. 


position  data  nee  errot  gap 


azimuth  arn>f  gap  (dag) 


Figure  13:  Difference  of  position  and  azimuth  errors  between  the  GED  and  TMA  methods 
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3.5.3  Sensitivity  to  Odometry  Errors 

The  TMA  method  requires  known  relative  geometry  between  two  successive  positions  in 
terms  of  £/l2,A#and  a  .  They  can  be  provided  by  either  odometry  or  inertial  navigation 
system  (INS).  As  for  odometry,  the  error  sources  include  mechanical  imperfections,  sensor 
errors,  and  wheel  skidding  or  slippage. 

For  indoor  applications,  after  calibration,  positional  accuracy  can  be  as  low  as  1-2%  of  distance 
travelled  [Papadopoulos,  E.,  and  Misailidis,  M.,  2007].  Setting  the  values  for  di2  and  a  errors 
should  reflect  such  positional  accuracy.  According  to  [Borenstein,  J.,  et  al,  1996],  A  0  error  can 
be  as  low  as  1-2°  on  smooth  surface  but  can  grow  up  to  8°  with  10  bumps.  This  error  growth 
can  be  prevented  if  a  gyroscope  is  incorporated  on  the  vehicle.  We  want  to  explore  the 
sensitivity  of  the  system  to  the  errors  associated  with  these  parameters.  We  vary  one 
parameter  at  a  time,  while  keeping  the  other  two  at  default  values  as  in  Subsection  3.5.1. 

First,  we  vary  the  dl2  error  from  1%  to  50%  of  the  distance  travelled  (ie.,  5m).  Therefore  2.5m 
(lcr)  would  be  the  largest  d]2  error  here.  After  3000  Monte-Carlo  simulations,  the  x/ y  position 
and  azimuth  errors  are  plotted  against  the  d]2  error  in  Figure  14.  The  red  curves  are  the 
smallest  possible  estimation  errors  with  4  bearing  measurements. 


■  pDlllisfl  ttilH 


2  J 


1  5 


0* 

, - . - . — - 

0  10  20  30  40  SO 


d12  •rrof  p*rc*nt«g« 


y  position  •nor 


2  S 


1  S 


OS 

0 - 1 - * - * - - - 

0  10  20  30  40  SO 


d12  ortor  porcontogo 


azimuth  •nor  (dog) 


d12  orror  porcontogo 


Figure  14:  Estimation  errors  versus  d]2  error  -  red  curves  are  the  smallest  possible  estimation  errors 
with  4  bearing  measurements 


It  is  shown  that  position  errors  remain  steady  for  dn  error  below  20%  and  increases  when  this 
error  exceeds  20%.  The  increase  in  the  displacement  errors  due  to  increase  in  the  d]2  error 
from  5%  to  50%  is  about  0.63m  (lcr) .  The  azimuth  accuracy  does  not  seem  to  be  affected  by 
d]  2  error. 

Next,  we  vary  Ad  error  from  1°  to  30°  while  keeping  the  rest  at  default.  The  results  shown  in 
Figure  15  indicate  that  the  performance  is  indeed  sensitive  to  A  6  error.  The  blue  curves  grow 
rapidly  from  the  start.  The  azimuth  error  exceeds  the  red  curve  at  2°,  and  the  position  error  (x 
and  y  combined)  exceeds  the  red  bound  at  12°  bearing  error  (not  shown  in  the  figure).  To  keep 
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this  error  source  under  control  in  outdoor  applications,  external  aiding  such  as  gyroscope  may 
be  necessary. 


dthete  error  (deg)  dthete  error  (deg)  dthete  mot  (deg) 

Figure  15:  Estimation  errors  versus  AO  error(\a )-  red  curves  are  the  smallest  possible  estimation 
errors  with  4  bearing  measurements 

Lastly,  we  vary  a  error  from  1°  to  30°  while  keeping  the  other  two  at  default.  According  to 
Figure  16,  a  error  contributes  to  position  estimation  errors  but  at  a  slower  pace  than  A# 
error  does.  The  azimuth  error  seems  unaffected  by  a  error.  It  appears  that  a  error  up  to  10° 
may  be  well  tolerated. 
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Figure  16:  Estimation  errors  versus  a  error  (\a)-  red  curves  are  the  smallest  possible  estimation 
errors  with  4  bearing  measurements 


From  the  above  observations,  it  is  clear  that  incremental  rotation  A  9  is  the  most  critical 
contributor  to  the  localisation  accuracy.  It  appears  that  A0  error  should  be  maintained  below 
2°.  This  can  be  achieved  as  numerous  low  cost  gyroscopes  in  the  market  meet  this 
requirement. 
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3.5.4  Sensitivity  to  Vehicle  Speed 

In  this  subsection,  we  examine  the  effect  of  distance  travelled  within  one  time  step  (ie.,  speed) 
on  the  estimation  accuracy.  The  vehicle  starts  from  (4, 10)  with  zero  azimuth.  It  moves  straight 
by  a  distance  of  dx2,  which  varies  from  0.1m  to  30m.  Just  for  this  test,  we  set  the  dX2,  A#  and 
a  errors  to  zero,  and  bearing  error  to  3°(lcr) . 
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Figure  17:  Estimation  errors  versus  distance  travelled  within  the  tune  step 

After  3000  Monte-Carlo  simulations,  the  position  and  azimuth  errors  are  plotted  versus  the 
distance  travelled  (ie.,  dn )  in  Figure  17.  The  estimation  errors  do  not  seem  to  vary  much  with 
dx2  •  The  position  error  is  larger  for  dX2  <  3  m,  and  then  remains  small  until  d{2  exceeds  16m. 
The  azimuth  error  increases  as  dl2  increases  but  the  change  is  not  significant.  Therefore,  we 
conclude  that  the  speed  variation  during  the  mission  does  not  affect  the  navigation  accuracy. 
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4.  Conclusions 

We  proposed  two  algorithms  for  estimating  the  position  and  orientation  of  a  mobile  robot 
(UGV)  given  noisy  bearing  measurements.  The  key  findings  of  the  study  are  listed  below. 

•  The  GED  method  finds  the  global  minimum  analytically  and  the  estimation  accuracy 
is  close  to  the  CRLB  for  bearing  errors  up  to  about  5°. 

•  If  the  bearing  error  becomes  large  (e.g.,  >5°),  few  iterations  of  MLE  optimisation  can  be 
added  to  bring  the  estimates  closer  to  the  correct  solution.  This  works  more  effectively 
when  N  is  large. 

•  The  performance  is  usually  more  accurate  when  the  vehicle  is  within  the  area 
delimited  by  the  landmarks. 

•  The  TMA  method  makes  use  of  additional  bearing  measurements  collected  at  the 
previous  unknown  location,  and  finds  the  current  and  previous  positions  (and 
azimuths)  simultaneously. 

•  The  TMA  method  results  in  a  significant  improvement (»  30%)  by  effectively  doubling 
the  number  of  the  measurements. 

•  The  TMA  method  requires  three  additional  terms,  dn,  AO  and  a ,  and  shows  greatest 
sensitivity  to  A#  error.  For  outdoor  applications,  incorporation  of  a  gyroscope  with  a 
bias  below  l°/s  is  recommended. 

•  The  vehicle  speed  does  not  significantly  affect  the  performance. 

These  methods,  especially  TMA,  can  be  combined  with  inertial  navigation  system  (INS) 
where  the  INS  can  provide  accurate  estimates  of  dn,A0  and  a ,  and  the  TMA  can 
prevent  the  INS  solution  from  drifting  via  Kalman  filtering. 

The  authors  are  currently  investigating  the  3D  localisation  problem,  where  the  vehicle 
(either  UAV  or  UGV)  has  a  moderate  tilt  and  takes  measurements  in  azimuth  and 
elevation. 
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Appendix  A:  Detailed  Derivation  of  TMA  Method 


Rotation  Matrices: 


K  = 


Rf  = 


D  M  — 
K52  “ 


cos  6X 

sin  9X 

-  sin  9X 

cos  9X 

cos  92 

sin  92 

-sin  92 
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cos  (A  9) 

sin(A#) 

l 

-  5 


^2 

-S 


REF  to  Robot  Position  1  (at  current  time) 


REF  to  Robot  Position  2  (at  previous  time) 


-  sin(A$)  cos(A#) 


2  2 
C 

-S 


Robot  Position  2  to  Robot  Positionl 


For  Robot  Position  1  (Current  Time):  Consider  i^  landmark  position  Li  wrt  B1  frame 

cos  9, 


Lm  = 


K  cos  A/ 
K  sin  A*. 


=  r*'(l,-bi)=r*'l)-r*'b1  = 


-  sin  9X  cos  9X 


sin  <9,  |  Xu 
Yu 


+  T, 


(Al) 


where  T.  1  = 


Yt 


=  -*?», 


Diving  the  top  row  by  bottom  row, 

cos  J3U  cos 0xXu  4  sin  9X  Yu  4  XT 
sin  /?,,  -  sin  9X Xu  4  cos  9X  Yu  4  Yr 


sin  fiXl(cos0xXu  +  s\n9xYu  +  XT)  -  cos  J3U(- sin  9xX Li  4cos  9XYU  4  Yr)  =  0 

cos#, 


[sin/?„X/( -cos puYIt,  sin PUYU  +  cos PXiXu ,  sin/?,,,  -cos/?,,]- 


sin  9i 
XT 
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=  0  (A2) 


Let 


^11,  =  sin  P\tX Li  -  cos  /?„  Yu 
A\2,  =  sin  fiuYu  +  cosfiuXu 


For  n  landmarks. 
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^11,  ,412,  sin/?,,  -cos/?,, 
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XT 
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For  Robot  Position  2  (previous  time  step):  Consider  i*  landmark  position  Li  wrt  B2  frame 
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Using  shorthand  notation  for  sine  and  cosine  functions. 
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Dividing  the  first  row  by  second  row. 
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sin  Pv  i(c  ’  c\  +  s  ’  a  +  {c *  s\  -s*c{  )YU  +  cXT  -sYr  +c-dnca  +  s-dx2sa] 
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The  highlighted  equations  for  B1  and  B2  can  be  combined  in  to 
Ax  =  b  where  A  is  2N  x  4 ,  x  is  4x1  and  b  is  2N  x  1  . 
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Appendix  B:  Maximum  Likelihood  Estimator 

As  mentioned  in  Section  2.6,  f(x)  is  a  nonlinear  vector  function.  To  determine  a  simple 
estimator,  f(x)  can  be  linearised  as  below 


f(x)«f(x0)  +  G-(x-x0) 

where  G  is  N  x3  matrix  of  derivatives  evaluated  at  xn 


G  = 


’  df 

df 

df 
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x  =  x0 

M  M  M 

dfN 
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ax(2) 

....  a*0) 

x=x0 

The  i[h  bearing  angle  is  expressed  as 

(  „  \ 


f  (x)  —  tan " 
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\Sx  J 
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g,  =  cos  0{XU  -  XB )  +  sin  0{YU  -  YB ) 
gy  =  “ sin 6(X ~XB)  +  cos 0(YU  -YH). 


Then  the  *th  row  elements  of  G  are 

df 
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df 
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(Bl) 


(B2) 


(B3) 


(B4) 

(B5) 

(B6) 


Inserting  (Bl)  into  the  cost  function  Q(x)  =  [p  -  f(x)J  N  '1  [p —  f(x)],  (where  N  is  a 
N  x  N  measurement  covariance  matrix). 


QW=[p-f(x0)-G(x-x0)]J  N~'[p-f(x0)-G(x-x0)]. 


(B  7) 
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To  minimise  Q(x),  we  want  =  03xl , 

dx 

=  2G  r  N  Gx  -  2G  r  N  (P  -  f(x0 )  +  Gx0 )  =  03xl  (B8) 

dx 

Rearranging  (B8)  gives  the  iterative  estimation  equation. 

x  =  x0+(G7'N'1G)',G7N',(p-f(x0)  (B9) 

And  if  we  assume  £[x]  =  x  (unbiased),  then  it  can  be  shown  that  the  covariance  matrix  of  x 
can  be  expressed  as 

P  =  £[(x  -  £[x])(x  -  £[x])7  ]  =  (g^N-'g)'1  .  (BIO) 

The  square-root  of  diagonals  of  P  become  the  Cramer  Rao  Lower  Bounds  for  the  estimate 
accuracy  of  x  =  [0,  X B ,  YB  ]7  . 
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Appendix  C:  Proof  of  Simultaneous  Diagonalisation 

Note  P  and  Q  are  real  symmetric,  and  P  is  positive  definite.  We  define  the  matrices  of 
eigenvectors  and  eigenvalues  of  Pas  [VP,DP]  =  eig(V)  where  eig  is  a  matlab  function  that 
produces  a  diagonal  matrix  Hp  of  eigenvalues  and  a  full  matrix  Vp  whose  columns  are  the 
corresponding  eigenvectors  so  that 

v;pvp  =  5:p.  (ci) 

We  define  a  transformation  matrix  that  will  diagonalise  P  and  Q  simultaneously  as  product 
of  two  matrices. 

S  =  S2S, .  (C2) 

We  set 

j 

S|  =  VpZp2  Vp  .  (C3) 

_2 

Note  that  £p2  exists  only  if  P  is  positive  definite  (ie.,  every  diagonal  elements  of  £P  are  positive). 

We  set  S2  to  be  the  transpose  of  the  eigenvector  matrix  of  Y  =  S,QSf  (ie,  S2  =  V7  where 
[VY»^-Y  ]  ~  eig(Y) ). 

Then  the  following  relationship  will  hold 

S2YS^  =  £y.  (C4) 

Note  that  eigenvector  matrices  are  always  orthogonal  therefore 

VPVP7'  =  Vp7  Vp  =  S2S72  =  S7S2  =  I4x4  (C5) 

Firstly,  we  need  to  prove  that  SPSy  =  I4x4 

SPS7  =  S2SIPS[S£ 

_!  I 

=  s2(vPzP2  v7  )(vpi:pv7  )(vpi:p2vp7  )S^ 

\  \_ 

=  S2Zp2rpZp2S2  (due  to  (C5)) 

=  S2S7' 

=  1 4x4  (due  t0  (C5)) 

Secondly,  we  need  to  prove  SQSr  =  Q„  where  Q„  is  diagonal. 

SQS7  =  S2S,QSfS2 

=  s2ys^ 

=  ZY  (diagonal  matrix  of  eigenvalues) 
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Appendix  D:  Lagrange  Multiplier  Boundary 

We  need  to  show  that  the  optimal  Lagrange  multiplier  must  be  upper  bounded  by  —  .  From 

Mx 

Section  3.4,  the  Langrangian  of  the  problem  is 


^(y>^)  =  yry  +  2g7y  +  M-A(yrQ0y-l) 
The  first  order  optimality  condition  was 
dl 


dy 


=  2(y  +  g  -  XQDy)  =  2((I  -  T*QD)y  +  g)  =  0 


The  second  order  optimality  condition  is 

d2L 


dy: 


=  (l-XQri)>0 


I-^‘Qd  = 


1-A>,  0  0  0 

0  1  -Xfi2  0  0 

0  0  10 

0  0  0  1 


>  04)(4  where  /r,  >  n2 


(Dl) 


(D2) 


(D3) 


(D4) 


From  (D4),  X  must  satisfy  both  X  <  1/  /r,  and  X  <  1/  /r2.  Since  //,  >  /r2,  by  taking  the 
intersection  of  the  two  boundary  conditions,  we  arrive  at  this  condition. 


X  <  1  /  //, 


(D5) 
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